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[00D3 ] However, in addition to it being necessary to provide detailed road map data inside 

the apparatus, since the detection results of this position detection apparatus contains 
error in the output values of the distance sensor and azimuth sensor, if position detection 
according to the output values of the distance sensor and azimuth sensor alone continues 
for a long period of time, the detection error ends up accumulating resulting in the 
problem of the detected vehicle position differing greatly from actual vehicle position. In 
addition, during movement of a humanoid robot that moves by the use of legs indoors in 
particular, although it is necessary for the robot itself to determine the relative locations 
m of walls and columns, the position detection apparatus of the prior art has the problem of 

Ll encountering difficulty in autonomous movement since it is difficult to determine relative 

H positional relationships with the surrounding environment. 

[ o(Jjj6 ] According to this aspect of the invention, since the same stationary object is 

JS:" extracted from the images of two consecutive frames as the reference characteristic point, 

the displacement of this stationary object is determined, and the amount of self- 
movement is determined from the amount of this displacement, the effect is obtained in 
which self-position can be detected with high accuracy. In addition, since extraction of 
the stationary object is performed autonomously, it is not necessary to provide a map and 
so forth in which the positions of stationary objects are pre-defined, thereby allowing the 
constitution to be simplified. Moreover, since it is not necessary to provide road map 
data or other map data, in addition to being able to simplify the constitution, it becomes 
possible to move to unknown locations, thereby obtaining the effect of being able to 
eliminate limitations on the range of action of the moving body. 



According to this aspect of the invention, since a position detection device is 
provided that detects self-position by substituting self-movement control and the 
observed amount of said reference point into an extended Kalman filter, the effect is 
obtained in which self-position can be detected more accurately. 

According to this aspect of the invention, since pre-stored object information and 
extracted characteristic points are compared, and those characteristic points having a high 
correlation to characteristic points in the pre-stored object information are considered to 
be known characteristic points that are used as reference characteristic points for 
calculating position, the effect is obtained in which self-position can be detected more 
accurately. 

According to this aspect of the invention, since said object information is updated 
by determining the relative relationship between unknown characteristic points and 
known characteristic points in an image in which characteristic points considered to be 
known are present, and storing said unknown characteristic points as known 
characteristic points, the effect is obtained in which object information can be updated 
automatically. 

According to this aspect of the invention, since self-position is determined by 
storing multiple characteristic point groups in an image pre-obtained using said image 
acquisition device with positions at which said characteristic point groups are obtained, 
and calculating the correlation between a characteristic point group of a newly obtained 



image and pre-stored characteristic point groups, the effect is obtained in which, even in 
cases in which robot position cannot be obtained geometrically, self-position can be 
detected based on previous results of position detection. 

[ 00 1 6 ] According to this aspect of the invention, since the same stationary object is 

extracted from the images of two consecutive frames as the reference characteristic point, 
the displacement of this stationary object is determined, and the amount of self- 
Cl movement is determined from the amount of this displacement, the effect is obtained in 

jjf* which self-position can be detected with high accuracy. In addition, since extraction of 

f\- the stationary object is performed autonomously, it is not necessary to provide a map and 

Q so forth in which the positions of stationary objects are pre-defined, thereby allowing the 

Q constitution to be simplified. Moreover, since it is not necessary to provide road map 

flj data or other map data, in addition to being able to simplify the constitution, it becomes 

U possible to move to unknown locations, thereby obtaining the effect of being able to 

eliminate limitations on the range of action of the moving body. 

[ 001 8 ] According to this aspect of the invention, since a position detection device is 

provided that detects self-position by substituting self-movement control and the 
observed amount of said reference point into an extended Kalman filter, the effect is 
obtained in which self-position can be detected more accurately. 
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According to this aspect of the invention, since pre-stored object information and 
extracted characteristic points are compared, and those characteristic points having a high 



correlation to characteristic points in the pre-stored object information are considered to 
be known characteristic points that are used as reference characteristic points for 
calculating position, the effect is obtained in which self-position can be detected more 
accurately. 

According to this aspect of the invention, since said object information is updated 
by determining the relative relationship between unknown characteristic points and 
known characteristic points in an image in which characteristic points considered to be 
known are present, and storing said unknown characteristic points as known 
characteristic points, the effect is obtained in which object information can be updated 
automatically. 

According to this aspect of the invention, since self-position is determined by 
storing multiple characteristic point groups in an image pre-obtained using said image 
acquisition device with positions at which said characteristic point groups are obtained, 
and calculating the correlation between a characteristic point group of a newly obtained 
image and pre-stored characteristic point groups, the effect is obtained in which, even in 
cases in which robot position cannot be obtained geometrically, self-position can be 
detected based on previous results of position detection. 

[0025] The invention according to an eleventh aspect is a position detection program for 

detecting the position of a moving object, said position detection program comprising 
performing by computer: image acquisition processing in which an image of the forward 
field of view of said moving object is acquired, distance image acquisition processing 
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having the same field of view as said image in which a distance image is acquired 
simultaneous to acquisition of said image, characteristic point extraction processing in 
which respective characteristic points are acquired from the images of at least two 
consecutive frames, and reference characteristic point selection processing in which the 
amount of displacement of a position between two frames of a characteristic point 
extracted in said characteristic point extraction processing is calculated based on said 
distance image, and a reference characteristic point for calculating position according to 
^ said amount of displacement is selected. 

[0Qp6 ] According to this aspect of the invention, since the same stationary object is 

I-"" extracted from the images of two consecutive frames as the reference characteristic point, 

the displacement of this stationary object is determined, and the amount of self- 
t% movement is determined from the amount of this displacement, the effect is obtained in 

m which self- position can be detected with high accuracy. In addition, since extraction of 

he* the stationary object is performed autonomously, it is not necessary to provide a map and 

so forth in which the positions of stationary objects are pre-defined, thereby allowing the 
constitution to be simplified. Moreover, since it is not necessary to provide road map 
data or other map data, in addition to being able to simplify the constitution, it becomes 
possible to move to unknown locations, thereby obtaining the effect of being able to 
eliminate limitations on the range of action of the moving body. 

[0027] The invention according to a twelfth aspect is a position detection program for 

detecting the position of a moving object, said position detection program comprising 
performing by computer: image acquisition processing in which an image within the 



forward field of view of said moving object is acquired, reference point determination 
processing in which a reference characteristic point to serve as a reference during 
movement of said moving object is determined based on said image, and position 
detection processing in which position is detected by substituting self-movement control 
and the observed amount of said reference point into an extended Kalman filter. 

According to this aspect of the invention, since a position detection device is 
provided that detects self-position by substituting self-movement control and the 
observed amount of said reference point into an extended Kalman filter, the effect is 
obtained in which self-position can be detected more accurately. 

[Of! 9 ] The invention according to a thirteenth aspect is a position detection program for 

%1 detecting the position of a moving object, said position detection program comprising 

performing by computer: image acquisition processing in which an image of the forward 
field of view of said moving object is acquired, distance image acquisition processing 
having the same field of view as said image in which a distance image is acquired 
simultaneous to acquisition of said image, characteristic point extraction processing in 
which respective characteristic points are extracted from obtained images, and reference 
characteristic point selection processing in which pre-stored object information is 
compared with extracted characteristic points, and those characteristic points having a 
high correlation are considered to be known characteristic points that are used as 
reference characteristic points for calculating position. 




According to this aspect of the invention, since pre-stored object information and 
extracted characteristic points are compared, and those characteristic points having a high 
correlation to characteristic points in the pre-stored object information are considered to 
be known characteristic points that are used as reference characteristic points for 
calculating position, the effect is obtained in which self-position can be detected more 
accurately. 

According to this aspect of the invention, since said object information is updated 
by determining the relative relationship between unknown characteristic points and 
known characteristic points in an image in which characteristic points considered to be 
known are present, and storing said unknown characteristic points as known 
characteristic points, the effect is obtained in which object information can be updated 
automatically. 

[0033 ] The invention according to a fifteenth aspect is a position detection program for 

detecting the position of a moving object, said position detection program comprising 
performing by computer: image acquisition processing in which an image of the forward 
field of view of said moving object is acquired, characteristic point group extraction 
processing in which a characteristic point group in said image is extracted, and position 
detection processing in which position is calculated by correlating and storing multiple 
characteristic point groups in an image pre-obtained in said image acquisition processing 
with positions at which said characteristic point groups are obtained, and calculating the 
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correlation between a characteristic point group of a newly obtained image and pre-stored 
characteristic point groups. 

According to this aspect of the invention, since self-position is determined by 
storing multiple characteristic point groups in an image pre-obtained using said image 
acquisition device with positions at which said characteristic point groups are obtained, 
and calculating the correlation between a characteristic point group of a newly obtained 
image and pre-stored characteristic point groups, the effect is obtained in which, even in 
cases in which robot position cannot be obtained geometrically, self-position can be 
detected based on previous results of position detection. 

DETAILED DESCRIPTION OF THE INVENTION 
Best Mode for Carrying Out the Invention 

First Embodiment 

The following provides an explanation of a position detection apparatus according 
to a first embodiment of the present invention with reference to the drawings. Fig. 1 is a 
block diagram showing the constitution of the present embodiment. Here, the position 
detection apparatus shown in Fig. 1 is explained in the form of that equipped on an 
autonomous travel robot that moves indoors. In this drawing, reference symbol 1 
represents two cameras that capture an object present in the field of view in the direction 
of movement during movement by the robot. These cameras are installed at a prescribed 
interval, and their mutual fields of view are aligned. Reference symbol 2 represents an 
image storage unit that respectively stores individual frames of images obtained by 
cameras 1, and is composed of two frames of image memory. Reference symbol 3 



represents a distance image formation unit that forms distance images from two frames of 
images stored in image storage unit 2. Reference symbol 4 represents a distance image 
storage unit that stores the distance images formed in distance image formation unit 3. 
Reference symbol 5 represents a characteristic point extraction unit that extracts 
characteristic points from images stored in image storage unit 2 or distance image storage 
unit 4. Reference symbol 6 represents a position detection unit that detects self-position 
based on the results of characteristic point extraction in characteristic point extraction 
unit 5. Reference symbol 7 represents a movement control unit that controls the 
S movement of the robot by referring to the results of position detection by position 

%l detection unit 6. 

[ 0(^7 ] Next, position detection unit 6 determines characteristic point distance from the 

G correlating points between each of the extracted reference characteristic points and the 

111 distance image stored in distance image storage unit 4, and calculates the amount the 

: M robot moved from the time at which the previous image was acquired to the time at which 

the current image (input image) was acquired based on the amount of movement of that 
characteristic point and the relative position of the robot (Step SI 7). When the 
processing of Step S17 is completed and the position of the robot is determined, the 
program returns again to Step SI and repeats the same processing on a characteristic 
point for which a correlation was previously obtained that has been stored in memory. 

[0072] In this manner, by continuously tracking the same stationary object from 

continuously incorporated images, the displacement of the position of the stationary 
object can be determined over time, and the amount of self-movement can be determined 



each time from the amount of this displacement, thereby enabling self-position to be 
detected accurately. Although the explanation here has dealt with the example of 
performing tracking processing with two consecutive images, in order to improve 
reliability and precision, tracking may be performed by determining the history of 
movement of a characteristic point from two or more images and extracting stationary 
objects therefrom. 

[0093 ] Here, an explanation is provided of the processing for calculating robot position 

rl in the case of using a Kalman filter. The Kalman filter used here is an extended Kalman 

m filter, and its status equation is shown in Equation (1). Here, if the variables that express 

P 1 the status of the robot are represented with x, y, $ and T, and defined as: 




then x t and y t are the x and y values on the x and y axes, § t is the angle of rotation about 
the z axis, and T t is the distance when moving from time step t to time step (t+1). 
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In Equation (1), the second term from the right is the amount of change in distance 
J{ control at time t from movement control unit 7, while the third term represents the system 

5 noise associated with robot control at time t. The coordinate system used here is the 

same as that of Fig. 4, while the meanings of each of the parameters that express the 
^ status of robot position are shown in Fig. 10. 

[Odlk] In addition > Equation (2) is the observation equation of characteristic point a, while 

m Equation (3) is the observation equation of characteristic point b. These equations 

E indicate the relative measured values of characteristic points a and b as viewed from the 

robot. This measurement equation is produced geometrically based on the coordinate 

system shown in Fig. 4. The measured amount is expressed with three variables 

consisting of x, y and G in the following manner: 

[0095] The above Equations (1) through (3) are used in the processing of Steps S25 and 

S29 shown in Fig. 6. By performing arithmetic operations on these equations each time 
an image is acquired, robot angle of rotation f+i and robot position (x t +i,y t +i) can be 
determined. By using this extended Kalman filter, robot position can be detected more 



accurately since both system noise and measurement noise are taken into consideration as 
compared with calculating geometrically as described above. In addition, when using a 
Kalman filter, although not specifically stated in the judgment of Step S28, the 
co variance of observed values relative to characteristic points that converges within the 
Kalman filter can be used as the variance values of those characteristic points. 

[0099 ] Fig. 14 indicates the example of there being 9 positions at which characteristic 

point groups are stored in memory (Ml through M9), these positions being preset at 
O positions over which the robot is likely to pass. If characteristic point groups extracted 

!£;f from an image captured with a camera facing in the direction of forward direction F-^ of 

Zl the robot from each preset position Ml through M9 are designated as Wl through W9, 

each position (referred to as a characteristic point acquisition position) Ml through M9 
O and characteristic point groups Wl through W9 are correlated into respective pairs which 

HI are then stored in advance in object data storage unit 8. When the robot is passing over 

an unknown position, the correlation between the characteristic point groups extracted 
from the image captured with the camera facing in the direction of forward direction F^ 
and pre-stored characteristic point groups Wl through W9 is determined, and 
characteristic point group Wn (where n is any of 1 through 9) having the highest 
correlation is judged to be the position over which the robot is passing, and the 
characteristic point acquisition position Mn (where n is any of 1 through 9) that forms a 
pair with this characteristic point group Wn becomes the self-position. For example, 
when characteristic point group W4 and a characteristic point group extracted from an 
image captured with the camera exhibit the closest correlation, position M4 at which this 
characteristic point group W4 was acquired is determined to be the self-position. 



[01 04 ] Next, self-position determination unit 10 selects the characteristic point group for 

which the sum S of certainty CA determined in Step 48 is largest (Step S52). Self- 
position is then determined by reading the characteristic point acquisition position that 
forms a pair with the selected characteristic point group from object data storage unit 8 
(Step S53) followed by output of that position as self-position data. 

[0106] In this manner, since characteristic point groups within the movable range of a 

O robot and positions at which those characteristic point groups were acquired are pre- 

stored in the robot, a characteristic point group having a high correlation with 

71 characteristic point groups extracted from an image obtained when the robot takes an 

Li action is selected from the characteristic point groups stored in memory, and the position 

O at which that selected characteristic point group was acquired is taken to be self-position, 

HI so that self-position can be detected easily in cases such as when a robot acts within a 

7 s .* predetermined room. 

[0111] As has been explained above, according to the present invention, since the same 

stationary object is extracted from the images of two consecutive frames, the 
displacement of the stationary object is determined and the amount of self-movement is 
determined from the amount of this displacement, whereby the present invention offers 
the advantage of being able to accurately detect self-position. In addition, since 
extraction of the stationary object can be performed independently, the constitution can 
be simplified since it is not necessary to provide a map and so forth in which the 
positions of stationary objects have been defined in advance. Moreover, since it is not 
necessary to provide a map such as road map data, in addition to it being possible to 



simplify the constitution, since the advantage is offered in which it becomes possible to 
move to an unknown location and eliminate restrictions on the range of action of a 
moving object, the present invention is particularly suited to a humanoid robot that moves 
by the use of legs. 

[0115] Although there have been described what are the present embodiments of the 

invention, it will be understood that changes and modifications may be made thereto 
without departing from the gist, spirit or essence of the invention. The scope of the 
if i invention is indicated by the appended claims. 



ABSTRACT OF THE DISCLOSURE 
A position detection apparatus that detects the position of a moving object, the 
position detection apparatus comprises a brightness image acquisition device that 
acquires a brightness image of the forward field of view of the moving object, a distance 
image acquisition device having the same field of view as the brightness image 
acquisition device that acquires a distance image simultaneous to acquisition of a 
brightness image by the brightness image acquisition device, a characteristic point 
extraction device that extracts respective characteristic points from the brightness images 
of at least two consecutive frames, and a reference characteristic point selection device 
that calculates the amount of displacement of a position between two frames of a 
characteristic point extracted by the characteristic point extraction device based on a 
distance image, and selects a reference characteristic point for calculating self-position 
according to said amount of displacement. 



the apparatus, since &is position detection apparatus contains error in the output values 



of the distance sensor and azimuth sensor, if position detection according to the output 
values of the distance sensor and azimuth sensor alone continues for a long period of 
time, the detection error ends up accumulating resulting in the problem of the detected 
vehicle position differing greatly from actual vehicle position. In addition, during 
movement of a humanoid robot that moves by the use of legs indoors in particular, 
although it is necessary for the robot itself to determine the relative locations of walls 
and columns, the position detection apparatus of the prior art has the problem of 
encountering difficulty in autonomous movement since it is difficult to determine 
relative positional relationships with the surrounding environment. 



Disclosure of the Invention 



In consideration of the circumstances as described above, the object of the present 
invention is to provide a position detection apparatus, position detection method and 
position detection program that makes it possible to easily detect self-position using 
images of the surrounding environment during autonomous movement by a humanoid 
robot that moves by the use of legs or automobile. 

The invention according to a first aspect is a position detection apparatus that 
detects the position of a moving object, said position detection apparatus being provided 
with an image acquisition device that acquires an image of the forward field of view of 
said moving object, a distance image acquisition device having the same field of view as 
said image acquisition device that acquires a distance image simultaneous to acquisition 
of an image by said image acquisition device, a characteristic point extraction device 
that extracts respective characteristic points from the images of at least two consecutive 
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frames, and a reference characteristic point selection device that calculates the amount of 
displacement of a position between two frames of a characteristic point extracted by said 
characteristic point extraction device based on said distance image, and selects a 
reference characteristic point for calculating self-position according to said amount of 



displacement. ^Jbjks/ 



According to thiS<5nvention, since the same stationary object is extracted from the 

j^X^^^-r-^^ w fV? J far, fMh tT 
images of two consecutive framessfthe displacement of this stationary object is 

determined, and the amount of self-movement is determined from the amount of this 

displacement, the effect is obtained in which self-position can be detected with high 

accuracy. In addition, since extraction of the stationary object is performed 

autonomously, it is not necessary to provide a map and so forth in which the positions of 

stationary objects are pre-defined, thereby allowing the constitution to be simplified. 

Moreover, since it is not necessary to provide road map data or other map data, in 

addition to being able to simplify the constitution, it becomes possible to move to 

unknown locations, thereby obtaining the effect of being able to eliminate limitations on 

the range of action of the moving body. 

The invention according to a second aspect is a position detection apparatus that 
detects the position of a moving object, said position detection apparatus being provided 
with an image acquisition device that acquires an image within the forward field of view 
of said moving object, a reference point determination device that determines a reference 
characteristic point to serve as a reference during movement of said moving object based 
on an image obtained from said image acquisition device, and a position detection 
device that detects position by substituting self-movement control and the observed 
amount of said reference point into an extended Kalman filter. 

According to thisvhrvenEonTsince a position detection device is provided that 
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detects self-position by substituting self-movement control and the observed amount of 
said reference point into an extended Kalman filter, the effect is obtained in which 
self-position can be detected more accurately. 

The invention according to a third aspect is a position detection apparatus that 
detects the position of a moving object, said position detection apparatus being provided 
with an image acquisition device that acquires an image of fee forward field of view of 
€ l said moving object, a distance image acquisition device having the same field of view as 

said image acquisition device that acquires a distance image simultaneous to acquisition 
N of an image by said image acquisition device, a characteristic point extraction device 

M : that extracts respective characteristic points from obtained images, and a reference 

O characteristic point selection device that compares pre-stored object information with 

pj extracted characteristic points, and considers those characteristic points having a high 

Uj 

CI correlation to be known characteristic points that are used as reference characteristic 

points for calculating position. 

According to this^ivention, since pre-stored object information and extracted 
characteristic points are compared, and those characteristic points having a high 
correlation^ are considered to be known characteristic points that are used as reference 
characteristic points for calculating position, the effect is obtained in which self-position 
can be detected more accurately 

In the invention according to a fourth aspect, said characteristic point selection 
device updates said object information by determining the relative relationship between 
unknown characteristic points and known characteristic points in an image in which 
characteristic points considered to be known are present, and storing said unknown 
characteristic points as known characteristic points. 

According to thisNinveHHon, since said object information is updated by 
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determining the relative relationship between unknown characteristic points and known 
characteristic points in an image in which characteristic points considered to be known 
are present, and storing said unknown characteristic points as known characteristic 
points, the effect is obtained in which object information can be updated automatically. 

The invention according to a fifth aspect is a position detection apparatus that 
detects the position of a moving object, said position detection apparatus being provided 
with an image acquisition device that acquires an image of the forward field of view of 
said moving object, a characteristic point group extraction device that extracts a 
characteristic point group in said image, and a position detection device that calculates 
position by correlating and storing multiple characteristic point groups in an image 
pre-obtained with said image acquisition device with positions at which said 
characteristic point groups are obtained, and calculating the correlation between a 
characteristic point group of a newly obtained image and pre-stored characteristic point 



According to tmk*ffivention, since self-position is fealculated by storing multiple 



characteristic point groups in an image pre-obtained(v^tfc( said image acquisition device 
with positions at which said characteristic point groups are obtained, and calculating the 
correlation between a characteristic point group of a newly obtained image and 
pre-stored characteristic point groups, the effect is obtained in which, even in cases in 
which robot position cannot be obtained geometrically, self-position can be detected 
based on previous results of position detection. 

The invention according to a sixth aspect is a position detection method that 
detects the position of a moving object, said position detection method having an image 
acquisition process in which an image of the forward field of view of said moving object 
is acquired, a distance image acquisition process having the same field of view as said 



groups. 
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image in which a distance image is acquired simultaneous to acquisition of said image, a 
characteristic point extraction process in which respective characteristic points are 
acquired from the images of at least two consecutive frames, and a reference 
characteristic point selection process in which the amount of displacement of a position 
between two frames of a characteristic point extracted in said characteristic point 
extraction process is calculated based on said distance image, and a reference 
characteristic point for calculating position according to said amount of displacement is 

selected. ^^4jt^ 

According to tMMnvention, since the same stationary object is extracted from the 
images of two consecutive frame^fffieHI^lacement of this stationary object is 
determined, and the amount of self-movement is determined from the amount of this 
displacement, the effect is obtained in which self-position can be detected with high 
accuracy. In addition, since extraction of the stationary object is performed 
autonomously, it is not necessary to provide a map and so forth in which the positions of 
stationary objects are pre-defined, thereby allowing the constitution to be simplified. 
Moreover, since it is not necessary to provide road map data or other map data, in 
addition to being able to simplify the constitution, it becomes possible to move to 
unknown locations, thereby obtaining the effect of being able to eliminate limitations on 
the range of action of the moving body. 

The invention according to a seventh aspect is a position detection method that 
detects the position of a moving object, said position detection method having an image 
acquisition process in which an image within the forward field of view of said moving 
object is acquired, a reference point determination process in which a reference 
characteristic point to serve as a reference during movement of said moving object is 
determined based on said image, and a position detection process in which position is 
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detected by substituting self-movement control and the observed amount of said 
reference point into an extended Kalman filter. 



detects self-position by substituting self-movement control and the observed amount of 
said reference point into an extended Kalman filter, the effect is obtained in which 
self-position can be detected more accurately. 

The invention according to an eighth aspect is a position detection method that 
detects the position of a moving object, said position detection method having an image 
acquisition process in which an image of the forward field of view of said moving object 
is acquired, a distance image acquisition process having the same field of view as said 
image in which a distance image is acquired simultaneous to acquisition of said image, a 
characteristic point extraction process in which respective characteristic points are 
extracted from obtained images, and a reference characteristic point selection process in 
which pre-stored object information is compared with extracted characteristic points, 
and those characteristic points having a high correlation are considered to be known 
characteristic points that are used as reference characteristic points for calculating 



According to thisnnvention, since pre-stored object information and extracted 
characteristic points are compared, and those characteristic points having a high 
correlationfere considered to be known characteristic points that are useJas reference 
characteristic points for calculating position, the effect is obtained in which self-position 
can be detected more accurately. 

In the invention according to a ninth aspect, the above characteristic point 
selection process updates said object information by determining the relative relationship 
between unknown characteristic points and known characteristic points in an image in 



According to thifelpv^^Si, since a position detection device is provided that 




position. 
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which characteristic points considered to be known are present, and storing said 

unknown characteristic points as known characteristic points. 

^$MAJt£^ 

According to MmnvSition, since said object information is updated by 

determining the relative relationship between unknown characteristic points and known 
characteristic points in an image in which characteristic points considered to be known 
are present, and storing said unknown characteristic points as known characteristic 
points, the effect is obtained in which object information can be updated automatically. 

The invention according to a tenth aspect is a position detection method that 
detects the position of a moving object, said position detection method having an image 
acquisition process in which an image of the forward field of view of said moving object 
is acquired, a characteristic point group extraction process in which a characteristic point 
group in said image is extracted, and a position detection process in which position is 
calculated by correlating and storing multiple characteristic point groups in an image 
pre-obtained in said image acquisition process with positions at which said characteristic 
point groups are obtained, and calculating the correlation between a characteristic point 
group of a newly obtained image and pre-stored characteristic point groups. 

According to tMsirlvSmon, since self-position is ^alculateaby storing multiple 
characteristic point groups in an image pre-obtained fedthj said image acquisition device 
with positions at which said characteristic point groups are obtained, and calculating the 
correlation between a characteristic point group of a newly obtained image and 
pre-stored characteristic point groups, the effect is obtained in which, even in cases in 
which robot position cannot be obtained geometrically, self-position can be detected 
based on previous results of position detection. 

The invention according to an eleventh aspect is a position detection program for 
detecting the position of a moving object, said position detection program comprising 
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s 

performing by computer image acquisition processing in which an image of the forward 

A 

field of view of said moving object is acquired, distance image acquisition processing 
having the same field of view as said image in which a distance image is acquired 
simultaneous to acquisition of said image, characteristic point extraction processing in 
which respective characteristic points are acquired from the images of at least two 
consecutive frames, and reference characteristic point selection processing in which the 
amount of displacement of a position between two frames of a characteristic point 
extracted in said characteristic point extraction processing is calculated based on said 
distance image, and a reference characteristic point for calculating position according to 
said amount of displacement is selected. 

According to tm^irrvSBwCsInce the same stationary object is extracted from the 
images of two consecutive framesjfffie displacement of this stationary objectis 

A 

determined, and the amount of self-movement is determined from the amount of this 

displacement, the effect is obtained in which self- position can be detected with high 

accuracy. In addition, since extraction of the stationary object is performed 

autonomously, it is not necessary to provide a map and so forth in which the positions of 

stationary objects are pre-defined, thereby allowing the constitution to be simplified. 

Moreover, since it is not necessary to provide road map data or other map data, in 

addition to being able to simplify the constitution, it becomes possible to move to 

unknown locations, thereby obtaining the effect of being able to eliminate limitations on 

the range of action of the moving body. 

The invention according to a twelfth aspect is a position detection program for 

detecting the position of a moving object, said position detection program comprising 
* 

performing by computer image acquisition processing in which an image within the 
forward field of view of said moving object is acquired, reference point determination 
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processing in which a reference characteristic point to serve as a reference during 
movement of said moving object is determined based on said image, and position 
detection processing in which position is detected by substituting self-movement control 
and the observed amount of said reference point into an extended Kalman filter. 

According to thijHnvention, since a position detection device is provided that 
detects self-position by substituting self-movement control and the observed amount of 
said reference point into an extended Kalman filter, the effect is obtained in which 
self-position can be detected more accurately. 

The invention according to a thirteenth aspect is a position detection program for 

detecting the position of a moving object, said position detection program comprising 

© 

0 

performing by computerimage acquisition processing in which an image of the forward 
field of view of said moving object is acquired, distance image acquisition processing 
having the same field of view as said image in which a distance image is acquired 
simultaneous to acquisition of said image, characteristic point extraction processing in 
which respective characteristic points are extracted from obtained images, and reference 
characteristic point selection processing in which pre-stored object information is 
compared with extracted characteristic points, and those characteristic points having a 
high correlation are considered to be known characteristic points that are used as 
reference characteristic points for calculating position. 

According to this^Hvention, since pre-stored object information and extracted 
characteristic points are compared, and those characteristic points having a high 
correlationVe^onsidered to be known characteristic points that are used as retereiice 
characteristic points for calculating position, the effect is obtained in which self-position 
can be detected more accurately. 

In the invention according to a fourteenth aspect, the above characteristic point 



11 

selection processing updates said object information by determining the relative 
relationship between unknown characteristic points and known characteristic points in 
an image in which characteristic points considered to be known are present, and storing 
said unknown characteristic points as known characteristic points. 

According to thi^rfivention, since said object information is updated by 
determining the relative relationship between unknown characteristic points and known 
characteristic points in an image in which characteristic points considered to be known 
are present, and storing said unknown characteristic points as known characteristic 
points, the effect is obtained in which object information can be updated automatically. 

The invention according to a fifteenth aspect is a position detection program for 

detecting the position of a moving object, said position detection program comprising 

<» 

performing by computer image acquisition processing in which an image of the forward 
field of view of said moving object is acquired, characteristic point group extraction 
processing in which a characteristic point group in said image is extracted, and position 
detection processing in which position is calculated by correlating and storing multiple 
characteristic point groups in an image pre-obtained in said image acquisition processing 
with positions at which said characteristic point groups are obtained, and calculating the 
correlation between a characteristic point group of a newly obtained image and 
pre-stored characteristic point groups. (fyjffyfc 



According to thismvention, since self-position is ^alculateayby storing multiple 



J 



characteristic point groups in an image pre-obtained (with] said image acquisition device 
with positions at which said characteristic point groups are obtained, and calculating the 
correlation between a characteristic point group of a newly obtained image and 
pre-stored characteristic point groups, the effect is obtained in which, even in cases in 
which robot position cannot be obtained geometrically, self-position can be detected 
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in the third embodiment. 

Fig. 14 is an explanatory drawing that explains the principle of position detection 
in the third embodiment. 

Fig. 1 5 is an explanatory drawing that explains the principle of position detection 
in the third embodiment. 

Best Mode for Carrying Out the Invention 

First Embodiment 

The following provides an explanation of a position detection apparatus according 
to a first embodiment of the present invention with reference to the drawings. Fig. 1 is 
a block diagram showing the constitution of the present embodiment. Here, the 
position detection apparatus shown in Fig. 1 is explained in the form of that equipped on 
an autonomous travel robot that moves indoors. In this drawing, reference symbol 1 
represents two cameras that capture an object present in the field of view in the direction 
of movement during movement by the robot. These cameras are installed at a 
prescribed interval, and their mutual fields of view are aligned. Reference symbol 2 
represents{a distancejimage storage unit that respectively stores individual frames of 
images obtained by cameras 1, andiscomposed of two frames of image memory. 
Reference symbol 3 represents a distance image formation unit that forms distance 
images from two frames of images stored in image storage unit 2. Reference symbol 4 
represents a distance image storage unit that stores the distance images formed in 
distance image formation unit 3. Reference symbol 5 represents a characteristic point 
extraction unit that extracts characteristic points from images stored in image storage 
unit 2 or distance image storage unit 4. Reference symbol 6 represents a position 
detection unit that detects self-position based on the results of characteristic point 
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stationary object (Step S 1 3). On the other hand, in the case the above calculated 
difference is greater than threshold value A, position detection unit 6 judges the 
characteristic point used when calculating the relevant displacement vector da to be a 
moving object (Step S 14). This processing is performed for all calculated displacement 
vectors da (Step SI 5) and as a result, a judgment is made as to whether a characteristic 
point is a stationary object or moving object for each characteristic point. 

Next, position detection unit 6 extracts those characteristic points judged to be 
stationary objects for which certainty CA is the superordinate (Step S 1 6). Here, 
certainty C A being superordinate refers to certainty CA being superordinate the smaller 
the value of CA. Those characteristic points extracted by this extraction processing are 
characteristic points of a previous image (time t) that form a pair with characteristic 
points of the input image (time t+l), and these characteristic points serve as the 
reference characteristic points for calculation of self-position. 

Next, position detection unit 6 determines characteristic point distance from the 
correlating points between each of the extracted reference characteristic points and the 
distance image stored in distance image storage unit 4, and calculates the amount the 
robot moved froii^me at which the previous image was acquired to the time at which 
the current image (input image) was acquired based on the amount of movement of that 
characteristic point and the relative position of the robot (Step S 1 7). When the 
processing of Step SI 7 is completed and the position of the robot is determined, the 
program returns again to Step S 1 and repeats the same processing on a characteristic 
point for which a correlation was previously obtained that has been stored in memory. 

Here, an explanation is provided of the principle for calculating self-position with 
reference to Fig. 4. The coordinate system here is a coordinate system defined when 
the robot is in the initial state, and the forward direction of the robot is designated as the 
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each time from the amount of this displacement, thereby enabling self-position to be 
detected accurately. Although the explanation here has dealt with the example of 
performing tracking processing with two consecutive images, in order to improve 
reliability and precision, tracking may be performed by determining the history of 
movement of a characteristic point from two or more images and extracting stationary 



In this manner, in the case the position of a target characteristic point is known, the 
absolute position of the robot can be determined, and in the case it is unknown, robot 
position (x t +i,y t +i) can be determined by determining the target position from the target 
relative position and robot position (x t ,y t ) at time t based on the amount of displacement 
of the position of that target at time t+1 • In addition, in the case of having continuously 
tracked the same target for a continuous amount of time K, the position at time t+k can 
be similarly detected based on the amount of displacement of (x t +ic ? yt+k) during time k. 

Furthermore, distance image formation unit 3 shown in Fig. 1 may be replaced 
with radar and so forth using ultrasonic waves or electromagnetic waves. At this time, 
the measuring field of view of the radar is set to be the same as that of cameras 1 shown 
in Fig. 1 . In addition, in the case of obtaining distance images using radar and so forth, 
only a single camera 1 should be provided for obtaining brightness images. 

In addition, in the case the moving body is moving, characteristic points may move 
outside the frame as the moving body approaches a characteristic point, thereby 
preventing detection of position. Consequently, characteristic points for which the 
amount of movement is calculated are successively switched by performing the 
processing shown in Fig. 2 on aUof a plurality of characteristic points. In this case, 
however, since the error in measured distance increases as the characteristic point moves 
away from the moving body s compared with the case of approaching the moving body, 
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objects U3 and U4, and adding the determined amount of movement to the relative 
coordinate values of point B (Step S29). In the case a known object is captured during 
the course of moving to point C, absolute self-position is determined based on that 
known object, and the error in relative position determined according to the amount of 
movement is reset. 

In this manner, in the case of determining absolute self-position based on the 
position of an unknown object, although the error in relative position obtained from a 
distance image accumulates, in the case a known object is captured, since self-position 
can be determined from this known object, the cumulative error can be reset, thereby 
enabling accurate position detection. 

Next, when again moving in the same area, in the case of moving to point A, since 
previously unknown objects Ul and U2 are now stored in internal memory as map 
position data and object data, they are used as known objects. In addition, this applies 
similarly to unknown objects U3 and U4. In addition, in the case a previously known 
object cannot be recognized to be present currently* since there is the possibility that the 
object is a moving object, processing may be added in which the possibility of that 
object being a moving object is stored in memory and removed from the detected 
objects. 

However, since variations in distance data obtained from images may be large, 
there are cases in which it is difficult to determine the position of^e robotjryjtie 
method described above. Consequently, robot position may be calculated using a 
Kalman filter based on extracted reference characteristic points. By using this Kalman 
filter, robot position can be calculated while taking into consideration variations in 
distance data and so forth. 

Here, an explanation is provided of the processing for calculating robot position 
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in the case of using a Kalman filter The Kalman filter used here is an extended 
Kalman filter, andjthe^its status equation is shown in Equation (1). Here, if the 
variables that express the status of the robot are represented with x, y, <f> and T, and 
defined as: 



y t 



then xt and y t are the x and y values on the x and y axes, <j> t is the angle of rotation about 
the 2 axis, and T t is the distance when moving from time step t to time step (t+1). 
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In Equation (1), the second term from the right is the amount of change in distance 
control at time t from movement control unit 7, while the third term represents the 
system noise associated with robot control at time t. The coordinate system used here 



is the same as that of Fig.0, while the meanings of each of the parameters that express 
the status of robot position are shown in Fig. 10. 

In addition, Equation (2) is the observation equation of characteristic point a, while 
Equation (3) is the observation equation of characteristic point b. These equations 
indicate the relative measured values of characteristic points a and b as viewed from the 
robot. This measurement equation is produced geometrically based on the coordinate 
system shown in Fig. (Sf. The measured amount is expressed with three variables 
consisting of x, y and 9 in the following manner: 



if 



y 
e 



is defined, then 



becomes 



y a 



(a x -x, ) cos {-<j> t ) - (a y -y, ) sin {-<f>, ) 
{a-x, ) sin (-$*, ) + {a y -y t ) cos {-<j>, ) 



0) 



ay 



...(2) 



f 



tan 



0) 



o8 



and 2 becomes 
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0, 



(b-x t )cos (-$*,) -(£,->>,) sin (-<*,) 
(b-x t ) sin (~<j> t ) + {b y ~y, ) cos {-</>, ) 

^3 
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bx 
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by 



...(3) 
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Furthennore, the following are also true: 



£, = (a- x, ) cos ) - (a y - y, ) sin ), 
E 2 = (a,- ) sin (-(j> t ) + (a y - y t ) cos (-^ ), 
E 3 = (6,- ) cos (-^, ) - y t ) sin (-$$, ), 
E 4 = (6,- ) sin (-^ ) + y, ) cos ), 



Here, the second term on the right side in Equations (2) and (3) represents noise. 

The above Equations (1) through (3) are used in the processing of Steps S25 and 
S29 shown in Fig. uL By performing arithmetic operations on these equations each time 
an image is acquired, robot angle of rotation <j> t +i and robot position (x t +i ? yt+0 can be 
determined. By using this extended Kalman filter, robot position can be detected more 
accurately since both system noise and measurement noise are taken into consideration 
as compared with calculating geometrically as described above. In addition, when 
using a Kalman filter, although not specifically stated in the judgment of Step S28 ; the 
covariance of observed values relative to characteristic points that converges within the 
Kalman filter can be used as the variance values of those characteristic points. 
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W are correlated with position M and stored in object data storage unit 8. Since the 
characteristic point groups here do not allow identification of their respective relative 
positions due to occlusion and so forth, stored positions are correlated for all 
characteristic points. 

Fig. 14 indicates the example of there being 9 positions at which characteristic 
point groups are stored in memory (Ml through M9), these positions being preset at 
positions over which the robot is likely to pass. If characteristic point groups extracted 
from image captured with a camera facing in the direction of forward direction F-» of 
the robot from each preset position Ml through M9 are designated as Wl through W9, 
each position (referred to as a characteristic point acquisition position) Ml through M9 
and characteristic point groups Wl through W9 are correlated into respective pairs 
which are then stored in advance in object data storage unit 8. When the robot is 
passing over an unknown position, the correlation between the characteristic point 
groups extracted from the image captured with the camera facing in the direction of 
forward direction F-> and pre-stored characteristic point groups Wl through W9 is 
determined, and characteristic point group Wn (where n is any of 1 through 9) having 
the highest correlation is judged to be the position over which the robot is passing, and 
the characteristic point acquisition position Mn (where n is any of 1 through 9) that 
forms a pair with this characteristic point group Wn becomes the self-position. For 
example, when characteristic point group W4 and a characteristic point group extracted 
from an image captured with the camera exhibit the closest correlation, position M4 at 
which this characteristic point group W4 was acquired is determined to be the 
self-position. 

Next, an explanation is provided of the specific procedure for determining 
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S42 through S48 consists of determining the certainty for all of the characteristic points 
that compose the characteristic point groups extracted from the input image as well as all 
of the characteristic points stored in object data storage unit 8. Furthermore, in the 
processing of Steps S44 through S50, the processing load can be reduced by determine 
certainty after selecting those characteristic pomtsiaclpse proximityto _tte image pixels. 

Next, self-position determination unit 10 selects the characteristic point group for 
which the sum S of certainty CA determined in Step 48 is large^Step S52). 
Self-position is then determined by reading the characteristic point acquisition position 
that forms a pair with the selected characteristic point group from object data storage 
unit 8 (Step S53) followed by output of that position as self-position data. 

In addition, as shown in Fig. 15, two left and right cameras that compose a stereo 
camera of the robot may be used to compare a pre-stored characteristic point group W 
for each camera and arithmetically determine correlation. When this is done, 
self-position can be determined according to the difference in the magnitudes of the 
degree of correlation of the camera images relative to the same characteristic point group 
W. Namely, the direction of offset to the left and right of position M can be detected 
from the difference in correlation values detected with each camera relative to position 
M corresponding to characteristic point group W. Consequently, since the number in 
the left and right directions of position M at which the characteristic point group is 
obtained can be reduced, the bother associated with capturing an image in advance can 
be reduced considerably. In addition, in the case there are two or more cameras that 
compose a stereo camera, they should be used by selecting cameras arranged in the left 
and right directions. 

In this manner, since characteristic point groups within the movable range of a 
robot and positions at which those characteristic point groups were acquired are 
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pre-stored in the robot, a characteristic point group having a high correlation with 
characteristic point groups extracted from an image obtained when the robot takes an 
action is selected from the characteristic point groups stored in memory, and the position 
at which that selected characteristic point group was acquired is taken to be self-position, 
self-position can be detected easily in cases such as when a robot acts within a 
predetermined room. 

Although the above Figs. 13 and 14 were explained assuming a single direction for 
the direction of image capturing at each position Ml through M9, if images in a plurality 
of directions are captured at each position, characteristic point groups are extracted for 
all resulting images, and those characteristic point groups are stored in object data 
storage unit 8, self-position can be detected by the same processing even if the forward 
direction of the robot differs at each position. In addition, the robot may also be made 
to extract characteristic point groups from enlarged or reduced images by using a zoom 
function of the camera equipped on the robot itself. If this is done, it becomes easier to 
obtain a correlation in a case such as when the robot is positioned between positions M2 
and M5 shown in Fig, 14. Moreover, the robot may also be made to extract 
characteristic point groups from images obtained in a state in which a head unit equipped 
with a camera swings horizontally and vertically. When this is done, it becomes easy 
to obtain a correlation between characteristic points even in cases in which the robot is 
acting in a direction different from the forward direction when it acquired a stored 
characteristic point group. 

In addition, the position detection processing previously described in the three 
embodiments may be suitably combined corresponding to the environment in which the 
robot moves, and self-position may be detected by selecting the necessary processing. 

In addition, a program for realizing the processing shown in Figs. 2, 6 and 12 may 
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determined from the amount of this displacement)™ present invention offers the 
advantage of being able to accurately detect self-position. In addition, since extraction 
of the stationary object can be performed independently, the constitution can be 
simplified since it is not necessary to provide a map and so forth in which the positions 
of stationary objects have been defined in advance. Moreover, since it is not necessary 
to provide a map such as road map data, in addition to it being possible to simplify the 
y?? constitution, since the advantage is offered in which it becomes possible to move to an 

fft unknown location and eliminate restrictions on the range of action of a moving object, 

y, the present invention is particularly suited to a humanoid robot that moves by the use of 

i legs. 

5™l In addition, according to the present invention, since a position detection device is 

5 = provided that detects self-position by substituting self-movement control and the 

5 J observed amount of the above reference point into an extended Kalman filter, the present 

invention offers the advantage of being able to detect self-position more accurately. 

In addition, according to the present invention, since pre-stored object data and 
extracted characteristic points are compared and characteristic points having a high 
degree of correlation are treated as known characteristic points and used as reference 
characteristic points for calculating self-position, the present invention offers the 
advantage of being able to detect self-position more accurately. 

In addition, according to the present invention, since object data is updated by 
determining the correlation between unknown characteristic points and known 
characteristic points in an image in which characteristic points treated as being known 
are present, and said unknown characteristic points are then used as known characteristic 
points and stored in memory, the present invention offers the advantage of enabling 
updating of map data and object data to be performed automatically. 
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ABSTRACT OF THE DISCLOSURE 
^The object of the present invention is to provide a position detection apparatus that 
is able to easily detect self-position during autonomous movement by a humanoid robot 
that moves by the use of legs or automobile. \ 

r Vr 

(The present invention discloses aj position detection apparatus that detects the 
position of a moving object, disposition detection apparatus/being provided witlv a 
brightness image acquisition device that acquires a brightness image of the forward field 
of view of the moving object, a distance image acquisition device having the same field 
of view as the brightness image acquisition device that acquires a distance image 
simultaneous to acquisition of a brightness image by the brightness image acquisition 
device, a characteristic point extraction device that extracts respective characteristic 
points from the brightness images of at least two consecutive frames, and a reference 
characteristic point selection device that calculates the amount of displacement of a 
position between two frames of a characteristic point extracted by the characteristic 
point extraction device based on a distance image, and selects a reference characteristic 
point for calculating self-position according to said amount of displacement. 



